/*** 
 * @Author: y.liu
 * @Date: 2023-09-15
 * @Description: 地图和路径消息发布类
 *  - 1. 地图消息发布
 *  - 2. 路径发布
 * @LastEditors: y.liu
 * @LastEditTime: 2023-08-31
 * @FilePath: global_plan_planner/src/path_pub.cpp
 */

#include "msg_publish.h"

//构造函数
MsgPublish::MsgPublish(){
    // 调试模式
    ros::param::param<bool>("/global_path_planner/debug_mode", __debug_Mode, true);
    //发布日志
    __log_Info_Pub = __nh.advertise<cln_msgs::LogMsg>("/log_save", 10);
    //全局路径发布
    __all_Cover_Path_Pub = __nh.advertise<geometry_msgs::PoseArray>("/debug_all_cover_data", 2);
    //发布p2p栅格地图数据
    __p2p_Map_Grid_Pub = __nh.advertise<nav_msgs::OccupancyGrid>("/debug_p2p_map_grid_dilate", 2, true);
    // 发布原始路径和优化后的路径
    __p2p_Path_origin_pub = __nh.advertise<nav_msgs::Path>("/debug_p2p_path_origin", 1, true);
    __p2p_Path_optimized_pub = __nh.advertise<nav_msgs::Path>("/debug_p2p_path_optimized", 1, true);
    //发布全覆盖地图膨胀数据
    __all_Map_Grid_Pub = __nh.advertise<nav_msgs::OccupancyGrid>("/debug_all_map_grid_dilate", 2, true);
}

// 析构函数
MsgPublish::~MsgPublish(){

}

// p2p膨胀地图发布
void MsgPublish::P2pMapGridDilatePub(cv::Mat& image, double& resolution, geometry_msgs::Pose& origin){
    if(!__debug_Mode) return;
    nav_msgs::OccupancyGrid __p2p_Map_Grid;
    __p2p_Map_Grid.info.height = image.rows;
    __p2p_Map_Grid.info.width = image.cols;
    __p2p_Map_Grid.info.resolution = resolution;
    __p2p_Map_Grid.info.origin = origin;
    __p2p_Map_Grid.info.map_load_time = ros::Time::now();

    //构建地图数据串（栅格地图变为单维度矩阵）
    __p2p_Map_Grid.data.resize(__p2p_Map_Grid.info.height * __p2p_Map_Grid.info.width);
    for(int row = 0; row < __p2p_Map_Grid.info.height; row++){
        for(int col = 0; col < __p2p_Map_Grid.info.width; col++)
        {
            int index = row * __p2p_Map_Grid.info.width + col;
            __p2p_Map_Grid.data[index] = image.at<uchar>(row, col);
        }
    }
    //设定地图参数数据
    __p2p_Map_Grid.header.frame_id = "/map";
    __p2p_Map_Grid.header.seq += 1;
    __p2p_Map_Grid.header.stamp = ros::Time::now();
    __p2p_Map_Grid_Pub.publish(__p2p_Map_Grid);
}

// 全覆盖膨胀地图发布
void MsgPublish::AllCoverMapGridDilatePub(cv::Mat& image, double& resolution, geometry_msgs::Pose& origin){
    if(!__debug_Mode) return;
    nav_msgs::OccupancyGrid __all_Map_Grid;
    __all_Map_Grid.info.height = image.rows;
    __all_Map_Grid.info.width = image.cols;
    __all_Map_Grid.info.resolution = resolution;
    __all_Map_Grid.info.origin = origin;
    __all_Map_Grid.info.map_load_time = ros::Time::now();

    //构建地图数据串（栅格地图变为单维度矩阵）
    __all_Map_Grid.data.resize(__all_Map_Grid.info.height * __all_Map_Grid.info.width);
    for(int row = 0; row < __all_Map_Grid.info.height; row++){
        for(int col = 0; col < __all_Map_Grid.info.width; col++)
        {
            int index = (__all_Map_Grid.info.height - row - 1) * __all_Map_Grid.info.width + col;
            __all_Map_Grid.data[index] = image.at<uchar>(row, col);
        }
    }
    //设定地图参数数据
    __all_Map_Grid.header.frame_id = "/map";
    __all_Map_Grid.header.seq += 1;
    __all_Map_Grid.header.stamp = ros::Time::now();
    __all_Map_Grid_Pub.publish(__all_Map_Grid);
}

// p2p原始路径发布
void MsgPublish::P2pPathOriginPub(std::vector<std::pair<double, double>>& path){
    if(!__debug_Mode) return;
    nav_msgs::Path res_path;
    res_path.header.frame_id = "map";
    res_path.header.stamp = ros::Time::now();
    for(auto it = path.begin(); it != path.end(); it++){
        geometry_msgs::PoseStamped pose_Stamped;
        pose_Stamped.header.stamp = ros::Time::now();
        pose_Stamped.header.frame_id = "map";
        pose_Stamped.pose.position.x = it->first;
        pose_Stamped.pose.position.y = it->second;
        res_path.poses.push_back(pose_Stamped);
    }
    __p2p_Path_origin_pub.publish(res_path);
}

// p2p优化路径发布
void MsgPublish::P2pPathOptimizedPub(std::vector<std::pair<double, double>>& path){
    if(!__debug_Mode) return;
    nav_msgs::Path res_path;
    res_path.header.frame_id = "map";
    res_path.header.stamp = ros::Time::now();
    for(auto it = path.begin(); it != path.end(); it++){
        geometry_msgs::PoseStamped pose_Stamped;
        pose_Stamped.header.stamp = ros::Time::now();
        pose_Stamped.header.frame_id = "map";
        pose_Stamped.pose.position.x = it->first;
        pose_Stamped.pose.position.y = it->second;
        res_path.poses.push_back(pose_Stamped);
    }
    __p2p_Path_optimized_pub.publish(res_path);
}

// 全覆盖路径发布
void MsgPublish::AllCoverPathPub(std::vector<std::vector<std::pair<float, float>>>& paths){
    if(!__debug_Mode) return;
    geometry_msgs::PoseArray pose_array;
    pose_array.header.frame_id = "map";
    pose_array.header.stamp = ros::Time::now();
    for(auto i = 0; i < paths.size(); i++){
        for(auto it = paths[i].begin(); it != paths[i].end(); it++){
            geometry_msgs::Pose pose;
            pose.position.x = it->first;
            pose.position.y = it->second;
            pose_array.poses.push_back(pose);
        }
    }
    __all_Cover_Path_Pub.publish(pose_array);
}

// P2P标准日志输出
std::string MsgPublish::P2PNormalLogInfo(geometry_msgs::Pose& start,
                                    geometry_msgs::Pose& goal,
                                    double& second, string& info,
                                    int point_size, bool& state){
    std::string msg = "";

    msg += "起点：[" + std::to_string(start.position.x) + ", " +
                    std::to_string(start.position.y) + ", " +
            "终点：[" + std::to_string(goal.position.x) + ", " +
                    std::to_string(goal.position.y) + "]，";
    msg += "生成路径点集：" + std::to_string(point_size) + "，";
    msg += "耗时：" + std::to_string(second) + "秒，规划状态：";

    if(state == 1) msg += "成功。";
    else msg += "失败。原因：" + info + ".";

    return msg;
}

// Full Cover Path标准日志输出
std::string MsgPublish::FCPPNormalLogInfo(geometry_msgs::Polygon polygon,
                                        double& second, string& info,
                                        int point_size, bool& state){
    std::string msg = "";

    msg += "轮廓点集：[";
    for(auto i = 0; i < polygon.points.size(); i++){
        msg += " [" + std::to_string(polygon.points[i].x) +
                ", " + std::to_string(polygon.points[i].y) +
                ", " + std::to_string(polygon.points[i].z) +
                "] ";
    }
    msg += "]";

    msg += ", 生成路径点集：" + std::to_string(point_size) +
            ", 耗时：" + std::to_string(second) + "秒" +
            "，规划状态：";

    if(state == 1) msg += "成功。";
    else msg += "失败。原因：" + info + ".";

    return msg;
}


// 任务日志保存
void MsgPublish::SaveLogInfo(string log_level, string msg, int log_code){
    vector<string> msgs;
    msgs.emplace_back(msg);
    LogInfoPub(log_level,msgs,log_code);
}

/*日志发布
*    @log_level 日志等级 DEBUG、INFO、WARN、ERROR、FATAL
*/
void MsgPublish::LogInfoPub(string log_level, vector<string> msgs, int log_code){
    cln_msgs::LogMsg log_msg;
    log_msg.node_name = ros::this_node::getName();
    log_msg.log_time = ros::Time::now();
    if(log_level == "DEBUG") log_msg.log_level = log_msg.DEBUG;
    else if (log_level == "INFO"){
        // for(auto message : msgs) ROS_INFO_STREAM("-->" << message.c_str() << "");
        log_msg.log_level = log_msg.INFO;
    }
    else if (log_level == "WARN"){
        // for(auto message : msgs) ROS_WARN_STREAM("-->" << message.c_str() << "");
        log_msg.log_level = log_msg.WARN;
    }
    else if (log_level == "ERROR"){
        // for(auto message : msgs) ROS_ERROR_STREAM("-->" << message.c_str() << "");
        log_msg.log_level = log_msg.ERROR;
    }
    else if (log_level == "FATAL"){
        // for(auto message : msgs) ROS_FATAL_STREAM("-->" << message.c_str() << "");
        log_msg.log_level = log_msg.FATAL;
    }
    log_msg.log_data = msgs;
    log_msg.log_code = log_code;
    __log_Info_Pub.publish(log_msg);
}